#include <iostream>
#include <Eigen/SVD>
#include <Eigen/Core>
#include <Eigen/Dense>

#include <sys/time.h>
#include <unistd.h>
#include <chrono>

#define MILLION 1000000L

// 设置矩阵行数、列数
#define ROW 6000//225
#define COL 300//15

using namespace std;


// 利用Eigen库，采用SVD分解的方法求解矩阵伪逆，默认误差er为0
Eigen::MatrixXd pinv_svd_based(Eigen::MatrixXd & M, const float er = 0) {
    // 进行svd分解
    Eigen::JacobiSVD<Eigen::MatrixXd> svd_holder(M,
                                                 Eigen::ComputeThinU |
                                                 Eigen::ComputeThinV);
    // 构建SVD分解结果
    Eigen::MatrixXd U = svd_holder.matrixU();
    Eigen::MatrixXd V = svd_holder.matrixV();
    Eigen::MatrixXd D = svd_holder.singularValues();

    // 构建S矩阵
    Eigen::MatrixXd S(V.cols(), U.cols());
    S.setZero();

    for (unsigned int i = 0; i < D.size(); ++i) {

        if (D(i, 0) > er) {
            S(i, i) = 1 / D(i, 0);
        } else {
            S(i, i) = 0;
        }
    }

    // pinv_matrix = V * S * U^T
    return V * S * U.transpose();
}

// 利用Eigen库，采用QR分解的方法求解矩阵伪逆
Eigen::MatrixXd pinv_qr_based(Eigen::MatrixXd & M) {
    // Eigen::MatrixXd R;
    // Eigen::MatrixXd Q;
    // qRDecomposition(M, Q, R);
    Eigen::HouseholderQR<Eigen::MatrixXd> qr;
    qr.compute(M);
    Eigen::MatrixXd R = qr.matrixQR().triangularView<Eigen::Upper>();
    Eigen::MatrixXd R1 = R.block(0,0,COL,COL);
    return R1.transpose() * (R1 * R1.transpose() + 0.00001*Eigen::MatrixXd::Identity(COL, COL)).inverse() * R1;
}





int main() {

    // 生成大小 ROW * COL 的随机矩阵
    // Eigen::Matrix<double, 225, 15> M;
    Eigen::MatrixXd M;
    M = Eigen::MatrixXd::Random(ROW, COL);

    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
	/********* target task **************/
    Eigen::MatrixXd PinvSVD = pinv_svd_based(M);
    /************************************/
	chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
	chrono::duration<double> time_used_svd = chrono::duration_cast<chrono::duration<double>>(t2 - t1)*MILLION;

    chrono::steady_clock::time_point t3 = chrono::steady_clock::now();
	/********* target task **************/
    Eigen::MatrixXd PinvQR = pinv_qr_based(M);
    /************************************/
	chrono::steady_clock::time_point t4 = chrono::steady_clock::now();
	chrono::duration<double> time_used_qr = chrono::duration_cast<chrono::duration<double>>(t4 - t3)*MILLION;

    cout <<"SOM矩阵维度：6000x300" << endl;
    cout <<"基于SVD客观性分析算法耗时： "<<time_used_svd.count()<<" us"<<endl;
    cout <<"新算法耗时： "<<time_used_qr.count()<<" us"<<endl;
    cout <<"效率提升： " << time_used_svd.count()/time_used_qr.count() << " 倍" << endl;
}
